/******************************************************************************
*	cmTask.c	Code file of CM Task Module.
*
*	coypright (C) 2009 Ding Lixing
*
******************************************************************************/

#include "cm.h"

#if CM_CFG_TASK_SLEEP_LEVEL == 2
    #define CM_CFG_TASK_SLEEP_MASK  (0x01)
#elif CM_CFG_TASK_SLEEP_LEVEL == 4
    #define CM_CFG_TASK_SLEEP_MASK  (0x03)
#elif CM_CFG_TASK_SLEEP_LEVEL == 8
    #define CM_CFG_TASK_SLEEP_MASK  (0x07)
#elif CM_CFG_TASK_SLEEP_LEVEL == 16
    #define CM_CFG_TASK_SLEEP_MASK  (0x0F)
#elif CM_CFG_TASK_SLEEP_LEVEL == 32
    #define CM_CFG_TASK_SLEEP_MASK  (0x1F)
#elif CM_CFG_TASK_SLEEP_LEVEL == 64
    #define CM_CFG_TASK_SLEEP_MASK  (0x3F)
#elif CM_CFG_TASK_SLEEP_LEVEL == 128
    #define CM_CFG_TASK_SLEEP_MASK  (0x7F)
#elif CM_CFG_TASK_SLEEP_LEVEL == 256
    #define CM_CFG_TASK_SLEEP_MASK  (0xFF)
#else
    #error CM_CFG_TASK_SLEEP_LEVEL: Look up!
#endif



#if CM_CFG_TASK_PRIO_LEVEL > 32
    #error CM_CFG_TASK_PRIO_LEVEL: Too big priority level!
#elif CM_CFG_TASK_PRIO_LEVEL > 24
    #define CM_TASK_GROUPS          (4)
#elif CM_CFG_TASK_PRIO_LEVEL > 16
    #define CM_TASK_GROUPS          (3)
#elif CM_CFG_TASK_PRIO_LEVEL > 8
    #define CM_TASK_GROUPS          (2)
#else
    #define CM_TASK_GROUPS          (1)
#endif


static cmLISTHEAD    cmTaskReadyLists[CM_CFG_TASK_PRIO_LEVEL + 1];
static cmLISTHEAD    cmTaskSleepLists[CM_CFG_TASK_SLEEP_LEVEL + 1];
static unsigned char cmTaskReadyMap[CM_TASK_GROUPS];
static cmTASK        *cmTaskReadyHigh;
static cmTASK        *cmTaskCurrent;
static unsigned int  volatile cmTickCounter = 0;
static cmTASK        idleTask;

static void cmTaskReadyListsInit(void)
{
    int i;

    for(i = 0; i < CM_CFG_TASK_PRIO_LEVEL + 1; i++){
        cmListHeadInit(&cmTaskReadyLists[i]);
    }

    for(i = 0; i < CM_TASK_GROUPS; i++){
        cmTaskReadyMap[i] = 0;
    }
}

static void cmTaskSleepListsInit(void)
{
    int i;

    for(i = 0; i < CM_CFG_TASK_SLEEP_LEVEL + 1; i++){
        cmListHeadInit(&cmTaskSleepLists[i]);
    }
}

static void cmTaskIdleInit(void)
{
    idleTask.slice = 0;
    idleTask.priority = CM_CFG_TASK_PRIO_LEVEL;
    idleTask.prioritySet = CM_CFG_TASK_PRIO_LEVEL;
    cmListAdd(&cmTaskReadyLists[CM_CFG_TASK_PRIO_LEVEL], &(idleTask.tNode));
}

static void cmTaskReadyMapSet(unsigned char priority)
{
    unsigned int group = (priority >> 3) & 3;
    cmTaskReadyMap[group] |= 1 << (priority & 7);
}

static void cmTaskReadyMapClr(unsigned char priority)
{
    unsigned int group = (priority >> 3) & 3;
    cmTaskReadyMap[group] &= ~(1 << (priority & 7));
}

static void cmTaskAddToReady(cmTASK *task)
{
    cmListAdd(&cmTaskReadyLists[task->priority], &task->tNode);
    cmTaskReadyMapSet(task->priority);
}

static void cmTaskDelFromReady(cmTASK *task)
{
    cmListNodeDel(&task->tNode);
    if(cmListIsEmpty(&cmTaskReadyLists[task->priority])){
        cmTaskReadyMapClr(task->priority);
    }
}

static void cmTaskReadyUpdate(void)
{
    static unsigned char hash[256] = 
    {
        0x0,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x5,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x6,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x5,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x7,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x5,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x6,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x5,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0,
        0x4,0x0,0x1,0x0,0x2,0x0,0x1,0x0,0x3,0x0,0x1,0x0,0x2,0x0,0x1,0x0
    };
    unsigned int highPrio;
    cmLISTNODE *highNode;

    highPrio = cmTaskReadyMap[0] ? hash[cmTaskReadyMap[0]] :
#if CM_CFG_TASK_PRIO_LEVEL > 8
               cmTaskReadyMap[1] ? 8 + hash[cmTaskReadyMap[1]] :
    #if CM_CFG_TASK_PRIO_LEVEL > 16
               cmTaskReadyMap[2] ? 16 + hash[cmTaskReadyMap[2]] :
        #if CM_CFG_TASK_PRIO_LEVEL > 24
               cmTaskReadyMap[3] ? 24 + hash[cmTaskReadyMap[3]] :
        #endif
    #endif
#endif
               CM_CFG_TASK_PRIO_LEVEL;

    
    highNode = cmListGetFirst(&cmTaskReadyLists[highPrio]);
    cmTaskReadyHigh = cmListNodeGetContainer(cmTASK, tNode, highNode);
}

void cmTaskSchedule(void)
{
    cmTaskReadyUpdate();
    if(cmTaskReadyHigh != cmTaskCurrent){
        if(!vmIsInSwInt() && !vmIsInInt()){
            vmSwIntRequest(CM_SW_SWITCHCTX, 0);
        }
    }
}

static void cmTaskAddToSleep(cmTASK *task)
{
    unsigned int sleepIndex;
    cmLISTHEAD *sleepHeader;
    
    if(task->sleepTicks != CM_SLEEP_FOREVER){
        sleepIndex = task->wakeCapture & CM_CFG_TASK_SLEEP_MASK;
    }else{
        sleepIndex = CM_CFG_TASK_SLEEP_LEVEL;
    }
    sleepHeader = &cmTaskSleepLists[sleepIndex];
    cmListAdd(sleepHeader, &task->tNode);
}

static void cmTaskDelFromSleep(cmTASK *task)
{
    cmListNodeDel(&task->tNode);
}

static vmVOID cmIntExitCallback(vmULONG ctxHandle)
{
    if(cmTaskReadyHigh != cmTaskCurrent){
        vmCtxGet(ctxHandle, &cmTaskCurrent->ctx);
        vmCtxSet(ctxHandle, &cmTaskReadyHigh->ctx);
        cmTaskCurrent = cmTaskReadyHigh;
    }
}

static vmVOID cmTickCallback(vmUINT8 irq, vmULONG param)
{
    cmTickCounter++;
    
    /* slice polling */
    if(cmTaskCurrent->slice){
        if(++(cmTaskCurrent->sliceConsumed) >= cmTaskCurrent->slice){
            cmTaskCurrent->sliceConsumed = 0;
            cmListShift(&cmTaskReadyLists[cmTaskCurrent->priority]);
        }
    }
    
    /* wakeup */
    {
        unsigned int sleepIndex = cmTickCounter & CM_CFG_TASK_SLEEP_MASK;
        cmLISTNODE *iter = cmListGetFirst(&cmTaskSleepLists[sleepIndex]);
        
        while(iter != cmListGetEnd(&cmTaskSleepLists[sleepIndex])){
            cmTASK *task = cmListNodeGetContainer(cmTASK,tNode,iter);
            iter = cmListNodeGetNext(iter);
            if(task->wakeCapture == cmTickCounter){
                cmTaskDelFromSleep(task);
                cmTaskAddToReady(task);
            }
        }
    }
    
    cmTaskSchedule();

    return;
}

static vmVOID cmCtxSwitchSwIsr(vmUINT8 swirq, vmULONG param)
{
    (void)swirq;
    (void)param;
    return;
}

static int cmExitCallback(int taskRet)
{
    cmTASK *self;
    
    (void)taskRet;

    vmIntLock();

    self = cmTaskCurrent;

    cmTaskDelFromReady(self);
    cmTaskFlagClr(self, CM_TFG_ALL);

    cmTaskSchedule();

    /* should not go here! */
    vmHalt();
    return 0;
}

void cmTaskLibInit(void)
{
    vmINTKEY key;


    key = vmIntLock();

    vmIntExitHandleRegister(cmIntExitCallback);
    vmSwIntExitHandleRegister(cmIntExitCallback);
    vmSwIrqConnect(CM_SW_SWITCHCTX, cmCtxSwitchSwIsr);
    vmTickSet(CM_CFG_TICKS_PER_SECOND, cmTickCallback);

    cmTaskReadyListsInit();
    cmTaskSleepListsInit();
    cmTaskIdleInit();
    cmTaskCurrent = &idleTask;
    cmTaskReadyHigh = &idleTask;

    vmIntUnlock(key);
}

int cmTaskInit
(
    cmTASK *        task,            /* Point of a task control block */
    unsigned char   priority,        /* Priority of task */
    unsigned char   slice,           /* Time slice of task */
    void *          stackAddr,       /* Stack address, four byte align */
    unsigned int    stackSize,       /* Stack size in byte */
    int             (*entry)(void *),/* Entry of task,  */
    void *          argument,        /* Argument of entry function of task */
    unsigned int    option           /* Option flag */
)
{
    (void)option;

    if(stackSize == 0 || priority >= CM_CFG_TASK_PRIO_LEVEL){
        return CM_ERR_InvalidArgument;
    }

    cmListNodeInit(&(task->tNode));
    cmListNodeInit(&(task->pNode));
    task->priority      = priority;
    task->prioritySet   = priority;
    task->slice         = slice;
    task->sliceConsumed = 0;
    task->sleepTicks    = 0;
    task->wakeCapture   = 0;
    task->exBlock       = (void *)0;
    task->entry         = entry;
    task->argument      = argument;
    task->exit          = (void *)cmExitCallback;
    task->stack         = stackAddr;
    task->stackSize     = stackSize;

    cmTaskFlagClr(task, CM_TFG_ALL);
    task->flagEx = 0;
    task->condType = CM_COND_WAIT_ANY;
    task->condWait = 0;
    task->reserved = 0;
    
    vmCtxInit(  &(task->ctx),
                (vmSTACK *)stackAddr, 
                (vmSTACK *)((vmSTACK*)stackAddr + stackSize / sizeof(vmSTACK)), 
                (vmVOID *)entry, 
                (vmVOID *)cmExitCallback,
                (vmULONG)argument,
                0);

    return 0;
}

int cmTaskStart(cmTASK *task)
{
    vmINTKEY key;

    key = vmIntLock();

    if(cmTaskFlagTst(task, CM_TFG_ACTIVE)){
        vmIntUnlock(key);
        return CM_ERR_StartingActiveTask;
    }
    cmTaskFlagSet(task, CM_TFG_ACTIVE);
    cmTaskAddToReady(task);

    cmTaskSchedule();

    vmIntUnlock(key);

    return 0;
}

void cmTaskToSleep(cmTASK *task, unsigned int tout)
{
    cmTaskDelFromReady(task);
    task->sleepTicks = tout;
    task->wakeCapture = cmTickCounter + tout;
    cmTaskFlagSet(task, CM_TFG_SLEEP);
    cmTaskAddToSleep(task);
}

void cmTaskSleep(unsigned int tout)
{
    if(tout != CM_SLEEP_NEVER){
        vmINTKEY key = vmIntLock();

        cmTaskToSleep(cmTaskCurrent, tout);
        cmTaskSchedule();

        vmIntUnlock(key);
    }
}

void cmTaskToWake(cmTASK *task)
{
    cmTaskDelFromSleep(task);
    cmTaskFlagClr(task, CM_TFG_SLEEP);
    cmTaskAddToReady(task);
}

int cmTaskWake(cmTASK *task)
{
    vmINTKEY key;

    key = vmIntLock();

    if(!cmTaskFlagTst(task, CM_TFG_SLEEP) || cmTaskFlagTst(task, CM_TFG_SUSPEND)){
        vmIntUnlock(key);
        return CM_ERR_WakeNosleepingTask;
    }
    cmTaskToWake(task);
    cmTaskSchedule();
    
    vmIntUnlock(key);

    return 0;
}

void cmTaskTransfer(void)
{
    vmINTKEY key;

    key = vmIntLock();
    
    cmTaskCurrent->sliceConsumed = 0;
    cmListShift(&cmTaskReadyLists[cmTaskCurrent->priority]);
    cmTaskSchedule();
    
    vmIntUnlock(key);

    return;
}

void cmTaskPriorityRuntimeSet(cmTASK *task, unsigned char priority)
{
    if(cmTaskFlagTst(task, CM_TFG_SLEEP)){
        if(cmTaskFlagTst(task, CM_OBJ_MASK)){
            /* a pend task */
            cmOBJ * pendItem = task->pendItem;
            if(pendItem->opt & CM_OP_PRIO){
                cmLISTNODE *iter;
                
                cmListNodeDel(&task->pNode);
                task->priority = priority;
                iter = cmListGetFirst(&pendItem->waitList);
                while(iter != cmListGetEnd(&pendItem->waitList)){
                    cmTASK *pos = cmListNodeGetContainer(cmTASK, pNode, iter);
                    if(pos->priority > task->priority){
                        break;
                    }
                    iter = cmListNodeGetNext(iter);
                }
                cmListNodeInsert(iter, &task->pNode);
            }else{
                task->priority = priority;
            }
        }else{
            /* a sleep task */
            task->priority = priority;
        }
    }else{
        /* a ready task */
        cmTaskDelFromReady(task);
        task->priority = priority;
        cmTaskAddToReady(task);
    }
}

int cmTaskPrioritySet(cmTASK *task, unsigned char priority)
{

    vmINTKEY key;

    if(priority >= CM_CFG_TASK_PRIO_LEVEL){
        return CM_ERR_InvalidArgument;
    }
    
    key = vmIntLock();
    if(!cmTaskFlagTst(task, CM_TFG_ACTIVE) || cmTaskFlagTst(task, CM_TFG_SUSPEND)){
        task->prioritySet = priority;
        task->priority = priority;
    }else{
        task->prioritySet = priority;
        cmTaskPriorityRuntimeSet(task, priority);

        cmTaskSchedule();
    }

    vmIntUnlock(key);

    return 0;
}

static void cmObjPendAddPriority(cmOBJ *obj, cmTASK *task)
{
    cmLISTNODE *iter = cmListGetFirst(&obj->waitList);
    
    
    while(iter != cmListGetEnd(&obj->waitList)){
        cmTASK *pos = cmListNodeGetContainer(cmTASK, pNode, iter);
        if(pos->priority > task->priority){
            break;
        }
        iter = cmListNodeGetNext(iter);
    }
    cmListNodeInsert(iter, &task->pNode);
}

#define cmObjPendAddFifo(obj, task)                                            \
cmListAdd(&(((cmOBJ *)(obj))->waitList), &(((cmTASK *)(task))->pNode))

static unsigned int cmObjUpdateWaitTick(cmTASK *task, unsigned int sleepTicks)
{
    if(sleepTicks != CM_SLEEP_FOREVER){
        unsigned int sleepPass;

        sleepPass = task->sleepTicks - task->wakeCapture + cmTickCounter;
        if(sleepPass > sleepTicks){
            /* Notice here */
            sleepTicks = CM_SLEEP_NEVER;
        }else{
            sleepTicks -= sleepPass;
        }
    }
    return sleepTicks;
}

int cmTaskObjInit
    (
    cmOBJ *obj,
    unsigned int type,
    unsigned int opt,
    int (*prePost)(cmOBJ *, void *),
    int (*prePend)(cmOBJ *, void *),
    void *data
    )
{
    if((type & ~CM_OBJ_MASK) || (opt & ~CM_OP_MASK)){
        return CM_ERR_InvalidArgument;
    }
    
    obj->type = type;
    obj->opt = opt;
    obj->prePend = prePend;
    obj->prePost = prePost;
    obj->data = data;
    cmListHeadInit(&obj->waitList);
    return 0;
}

int cmTaskObjPost(cmOBJ *obj, void *param)
{
    int ret;
    vmINTKEY key;

    key = vmIntLock();
    ret = obj->prePost(obj, param);
    if(ret == 0){
        if(!cmListIsEmpty(&obj->waitList)){
            cmLISTNODE *nodeWait;
            cmTASK *taskWait;
            
            nodeWait = cmListGetFirst(&obj->waitList);
            taskWait = cmListNodeGetContainer(cmTASK, pNode, nodeWait);
            cmListNodeDel(nodeWait);
            cmTaskToWake(taskWait);
            cmTaskFlagClr(taskWait, CM_OBJ_MASK);
            cmTaskSchedule();
        }
    }
    vmIntUnlock(key);
    return ret;
}

int cmTaskObjPostEach(cmOBJ *obj, void *param, int(*callback)(cmOBJ *, cmTASK*))
{
    int ret;
    vmINTKEY key;

    key = vmIntLock();
    ret = obj->prePost(obj, param);
    if(ret == 0){
        cmLISTNODE *iter = cmListGetFirst(&obj->waitList);
        while(iter != cmListGetEnd(&obj->waitList)){
            cmLISTNODE *nodeWait = iter;
            cmTASK *taskWait;

            iter = cmListNodeGetNext(iter);
            taskWait = cmListNodeGetContainer(cmTASK, pNode, nodeWait);
            if(0 == callback(obj, taskWait)){
                cmListNodeDel(nodeWait);
                cmTaskToWake(taskWait);
                cmTaskFlagClr(taskWait, CM_OBJ_MASK);
            }
        }
        cmTaskSchedule();
    }
    vmIntUnlock(key);
    return ret;
}

int cmTaskObjPend(cmOBJ *obj, unsigned int tout, void *param)
{
    int ret;
    unsigned int sleepTicks = tout;
    vmINTKEY key;

    key = vmIntLock();
    
    for(;;){
        ret = obj->prePend(obj, param);
        if(ret != 0){
            cmTASK *self = cmTaskCurrent;
            
            if(sleepTicks == CM_SLEEP_NEVER){
                cmTaskFlagClr(self, CM_OBJ_MASK);
                break;
            }else{
                if(obj->opt & CM_OP_PRIO){
                    cmObjPendAddPriority(obj, self);
                }else{
                    cmObjPendAddFifo(obj, self);
                }
                cmTaskToSleep(self, sleepTicks);

                self->pendItem = (void *)obj;
                cmTaskFlagSet(self, obj->type);
                cmTaskSchedule();
                sleepTicks = cmObjUpdateWaitTick(self, sleepTicks);
            }
        }else{
            break;
        }
    }
    vmIntUnlock(key);
    
    return ret;
}

int cmTaskSuspend(cmTASK *task)
{
    int ret = 0;
    vmINTKEY key;

    key = vmIntLock();
    
    if(!cmTaskFlagTst(task, CM_TFG_ACTIVE) || cmTaskFlagTst(task, CM_TFG_SUSPEND)){
        ret = -1;
    }else{
        if(cmTaskFlagTst(task, CM_TFG_SLEEP)){
            cmTaskDelFromSleep(task);
            task->sleepTicks = cmObjUpdateWaitTick(task, task->sleepTicks);
            if(cmTaskFlagTst(task, CM_OBJ_MASK)){
                cmListNodeDel(&task->pNode);
            }
        }else{
            cmTaskDelFromReady(task);
        }
    }
    vmIntUnlock(key);
    
    return ret;
}

int cmTaskResume(cmTASK *task)
{
    int ret = 0;
    vmINTKEY key;

    key = vmIntLock();
    
    if(!cmTaskFlagTst(task, CM_TFG_ACTIVE) || !cmTaskFlagTst(task, CM_TFG_SUSPEND)){
        ret = -1;
    }else{
        if(cmTaskFlagTst(task, CM_TFG_SLEEP)){
            task->wakeCapture = task->sleepTicks + cmTickCounter;
            cmTaskAddToSleep(task);
            if(cmTaskFlagTst(task, CM_OBJ_MASK)){
                cmOBJ *obj = (cmOBJ *)task->pendItem;
                if(obj->opt & CM_OP_PRIO){
                    cmObjPendAddPriority(obj, task);
                }else{
                    cmObjPendAddFifo(obj, task);
                }
            }
        }else{
            cmTaskAddToReady(task);
        }
        cmTaskSchedule();
    }
    vmIntUnlock(key);

    return ret;
}

cmTASK *cmTaskGetCurrent(void)
{
    return cmTaskCurrent;
}

